//
// Created by liangwenhao on 2024/4/4.
//

#include "bsp_can.h"
CAN_Receive can1_receive;
CAN_Receive can2_receive;
/*------------------------------------------------can过滤器初始化------------------------------------------------------------------*/
//CubeMx不会生成过滤器配置相关函数，需要自己写
uint8_t UserCan1FilterConfig(void)//初始化can1过滤器
{
    CAN_FilterTypeDef  CAN1_FilerConf;
    CAN1_FilerConf.FilterIdHigh=0x0000;     //32 bit ID
    CAN1_FilerConf.FilterIdLow=0x0000;                     //High                        Low
    CAN1_FilerConf.FilterMaskIdHigh=0x0000; //32 bit MASK 0000 0010 0000 0xxx EXID[5] 0000 0000 0000 0000
    //               1111 1111 1111 0000         1111 1111 1111 1111
    CAN1_FilerConf.FilterMaskIdLow=0x0000;
    CAN1_FilerConf.FilterFIFOAssignment=CAN_FILTER_FIFO0;//FIFO0 with filter
    CAN1_FilerConf.FilterBank=0;          //filter 0
    CAN1_FilerConf.FilterMode=CAN_FILTERMODE_IDMASK;
    CAN1_FilerConf.FilterScale=CAN_FILTERSCALE_32BIT;
    CAN1_FilerConf.FilterActivation=ENABLE; //enable filter
    CAN1_FilerConf.SlaveStartFilterBank=14;

    if(HAL_CAN_ConfigFilter(&hcan1,&CAN1_FilerConf)!=HAL_OK) return 1;//initialize filter
    if(HAL_CAN_Start(&hcan1)!=HAL_OK) return 1;//start can
    if(HAL_CAN_ActivateNotification(&hcan1,CAN_IT_RX_FIFO0_MSG_PENDING)!=HAL_OK) return 1;//enable ix0 handler
    return 0;
}

uint8_t UserCan2FilterConfig(void)//初始化can2过滤器
{
    CAN_FilterTypeDef  CAN2_FilerConf;
    CAN2_FilerConf.FilterIdHigh= 0x0000;
    CAN2_FilerConf.FilterIdLow=0x0000;
    CAN2_FilerConf.FilterMaskIdHigh=0x0000;
    CAN2_FilerConf.FilterMaskIdLow=0x0000;
    CAN2_FilerConf.FilterFIFOAssignment=CAN_FILTER_FIFO1;
    CAN2_FilerConf.FilterBank=14;
    CAN2_FilerConf.FilterMode=CAN_FILTERMODE_IDMASK;
    CAN2_FilerConf.FilterScale=CAN_FILTERSCALE_32BIT;
    CAN2_FilerConf.FilterActivation=ENABLE;
    CAN2_FilerConf.SlaveStartFilterBank=4;

    if(HAL_CAN_ConfigFilter(&hcan2,&CAN2_FilerConf)!=HAL_OK) return 1;
    if(HAL_CAN_Start(&hcan2)!=HAL_OK) return 1;
    if(HAL_CAN_ActivateNotification(&hcan2,CAN_IT_RX_FIFO1_MSG_PENDING)!=HAL_OK) return 1;
    return 0;
}

/*------------------------------------------------CAN1和CAN2的接收函数----------------------------------------------------------------*/
uint8_t CAN1_Receive_Msg(CAN_Receive * can_receive)//can1接收
{
    CAN_RxHeaderTypeDef RxMessage;
    if( HAL_CAN_GetRxFifoFillLevel(&hcan1, CAN_RX_FIFO0)==0)
    {
        return 0;//没有接收到数据,直接退出
    }
    HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &RxMessage, can_receive->ReceiveData);//数据接收
    can_receive->ReceiveID=RxMessage.StdId;	//ID接收
    return RxMessage.DLC; //返回接受到的数据长度
}

uint8_t CAN2_Receive_Msg(CAN_Receive * can_receive)//can2接收
{
    CAN_RxHeaderTypeDef RxMessage;
    if( HAL_CAN_GetRxFifoFillLevel(&hcan2, CAN_RX_FIFO1)==0)
    {
        return 0;
    }
    HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &RxMessage, can_receive->ReceiveData);
    can_receive->ReceiveID=RxMessage.StdId;
    return RxMessage.DLC; //返回接受到的数据长度
}


/*------------------------------------------------CAN1和CAN2发送函数-------------------------------------------------------------------*/
uint8_t CAN1_Send_Msg(CAN_Send * can_send,uint8_t len)//can1发送
{
    CAN_TxHeaderTypeDef TxMessage;
    uint32_t tx_mailbox;
    uint16_t i=0;
    uint8_t Data[8];
    TxMessage.StdId=can_send->SendID;        //standard ID 0x1FF/0x200
    TxMessage.ExtId=0x00;        //extended ID (29bit)
    TxMessage.IDE=CAN_ID_STD;
    TxMessage.RTR=CAN_RTR_DATA;
    TxMessage.DLC=len;
    for(i=0;i<len;i++){
        Data[i]=can_send->SendData[i];
    }
    if(HAL_CAN_AddTxMessage(&hcan1,&TxMessage,Data,&tx_mailbox)==HAL_OK)
    {
        __NOP();
        return 1;
    }else{
        __NOP();
        return 0;
    }
}

uint8_t CAN2_Send_Msg(CAN_Send * can_send,uint8_t len)//can2发送
{
    CAN_TxHeaderTypeDef TxMessage;
    uint16_t i=0;
    uint8_t Data[8];
    TxMessage.StdId=can_send->SendID;
    TxMessage.ExtId=0x12;
    TxMessage.IDE=CAN_ID_STD;
    TxMessage.RTR=CAN_RTR_DATA;
    TxMessage.DLC=len;
    for(i=0;i<len;i++)
        Data[i]=can_send->SendData[i];
    if(HAL_CAN_AddTxMessage(&hcan2,&TxMessage,Data,(uint32_t *)CAN_TX_MAILBOX1)!=HAL_OK)
    {
        return 1;
    }
    return 0;
}

/*-----------------------------------------CAN1和CAN2发送接收数组初始化----------------------------------------------------------------------------*/
void CAN_Receive_Init(CAN_Receive * can_receive)//can接收数组初始化
{
    can_receive->ReceiveID=0;
    for(int i=0;i<8;i++)
    {
        can_receive->ReceiveData[i]=0;
    }
}

void CAN_Send_Init(CAN_Send * can_send)//can发送数组初始化
{
    can_send->SendID =0x200;
    for(int i=0;i<8;i++)
    {
        can_send->SendData[i]=0;
    }
}


/*-------------------------------------CAN过滤器初始化和启动端口------------------------------------------------------------------*/
void CAN12_FilterInitAndStart(void)
{
    UserCan1FilterConfig();
    UserCan2FilterConfig();
}

//float Master_angle_pre = 0,Master_angle_now = 0;
//float Master_angle_cross = 0;

void CAN1_INTERRUPT(CAN_Receive *can_receive)//应用在can1中断中
{
    CAN1_Receive_Msg(can_receive);
    switch(can_receive->ReceiveID)
    {
        case 0x201:
        {
            Motor_canRx_calculation(&LoadMotor1,can_receive->ReceiveData);
            break;
        }
        case 0x203:
        {
            Motor_canRx_calculation(&LoadMotor2,can_receive->ReceiveData);
            break;
        }
        case 0x204:
        {
            Motor_canRx_calculation(&YawMotor,can_receive->ReceiveData);
            break;
        }
    }
}

void CAN2_INTERRUPT(CAN_Receive *can_receive)//应用在can2中断中
{
    CAN2_Receive_Msg(can_receive);
    switch(can_receive->ReceiveID)
    {
        case 0x202:
        {
            Motor_canRx_calculation(&LocateMotor,can_receive->ReceiveData);
            break;
        }
        case 1:
        {
            uint16_t low,high;

            low=(uint16_t)(can_receive->ReceiveData[4]<<8|can_receive->ReceiveData[3]);
            high=(uint16_t)(can_receive->ReceiveData[6]<<8|can_receive->ReceiveData[5]);
            YawEncoder_Value=ABSOLUTE_ENCODER_MAX-(float)((uint32_t)(high<<16|low));
            YawEncoder_Real_Angle= YawEncoder_Value/ABSOLUTE_ENCODER_MAX;
            break;
        }
    }
}

//不使用信号量，单纯使用中断，CAN1挂FIFO0,CAN2挂FIFO1